// Astar.cpp
// http://en.wikipedia.org/wiki/A*
// Compiler: Dev-C++ 4.9.9.2
// FB - 201012256
#include "SFML\Graphics.hpp"
#include <iostream>
#include <iomanip>
#include <queue>
#include <string>
#include <math.h>
#include <ctime>
using namespace std;

namespace ns_path_finder
	{
	const int n=32; // horizontal size of the map
	const int m=32; // vertical size size of the map
	static int map[n][m];
	static int closed_nodes_map[n][m]; // map of closed (tried-out) nodes
	static int open_nodes_map[n][m]; // map of open (not-yet-tried) nodes
	static int dir_map[n][m]; // map of directions
	const int dir=4; // number of possible directions to go at any position
	// if dir==4
	static int dx[dir]={1, 0, -1, 0};
	static int dy[dir]={0, 1, 0, -1};
	// if dir==8
	//static int dx[dir]={1, 1, 0, -1, -1, -1, 0, 1};
	//static int dy[dir]={0, 1, 1, 1, 0, -1, -1, -1};

	class node
	{
		// current position
		int xPos;
		int yPos;
		// total distance already travelled to reach the node
		int level;
		// priority=level+remaining distance estimate
		int priority;  // smaller: higher priority

		public:
			node(int xp, int yp, int d, int p) 
				{xPos=xp; yPos=yp; level=d; priority=p;}
    
			int getxPos() const {return xPos;}
			int getyPos() const {return yPos;}
			int getLevel() const {return level;}
			int getPriority() const {return priority;}

			void updatePriority(const int & xDest, const int & yDest)
			{
				 priority=level+estimate(xDest, yDest)*10; //A*
			}

			// give better priority to going strait instead of diagonally
			void nextLevel(const int & i) // i: direction
			{
				 level+=(dir==8?(i%2==0?10:14):10);
			}
        
			// Estimation function for the remaining distance to the goal.
			const int & estimate(const int & xDest, const int & yDest) const
			{
				static int xd, yd, d;
				xd=xDest-xPos;
				yd=yDest-yPos;         

				// Euclidian Distance
				d=static_cast<int>(sqrt(double(xd*xd+yd*yd)));

				// Manhattan distance
				//d=abs(xd)+abs(yd);
            
				// Chebyshev distance
				//d=max(abs(xd), abs(yd));

				return(d);
			}
	};

	// Determine priority (in the priority queue)
	bool operator<(const node & a, const node & b)
	{
	  return a.getPriority() > b.getPriority();
	}

	// A-star algorithm.
	// The route returned is a string of direction digits.
	string pathFind( const int & xStart, const int & yStart, 
					 const int & xFinish, const int & yFinish )
	{
		static priority_queue<node> pq[2]; // list of open (not-yet-tried) nodes
		static int pqi; // pq index
		static node* n0;
		static node* m0;
		static int i, j, x, y, xdx, ydy;
		static char c;
		pqi=0;

		// reset the node maps
		for(y=0;y<m;y++)
		{
			for(x=0;x<n;x++)
			{
				closed_nodes_map[x][y]=0;
				open_nodes_map[x][y]=0;
			}
		}

		// create the start node and push into list of open nodes
		n0=new node(xStart, yStart, 0, 0);
		n0->updatePriority(xFinish, yFinish);
		pq[pqi].push(*n0);
		open_nodes_map[x][y]=n0->getPriority(); // mark it on the open nodes map

		// A* search
		while(!pq[pqi].empty())
		{
			// get the current node w/ the highest priority
			// from the list of open nodes
			n0=new node( pq[pqi].top().getxPos(), pq[pqi].top().getyPos(), 
						 pq[pqi].top().getLevel(), pq[pqi].top().getPriority());

			x=n0->getxPos(); y=n0->getyPos();

			pq[pqi].pop(); // remove the node from the open list
			open_nodes_map[x][y]=0;
			// mark it on the closed nodes map
			closed_nodes_map[x][y]=1;

			// quit searching when the goal state is reached
			//if((*n0).estimate(xFinish, yFinish) == 0)
			if(x==xFinish && y==yFinish) 
			{
				// generate the path from finish to start
				// by following the directions
				string path="";
				while(!(x==xStart && y==yStart))
				{
					j=dir_map[x][y];
					c='0'+(j+dir/2)%dir;
					path=c+path;
					x+=dx[j];
					y+=dy[j];
				}

				// garbage collection
				delete n0;
				// empty the leftover nodes
				while(!pq[pqi].empty()) pq[pqi].pop();           
				return path;
			}

			// generate moves (child nodes) in all possible directions
			for(i=0;i<dir;i++)
			{
				xdx=x+dx[i]; ydy=y+dy[i];

				if(!(xdx<0 || xdx>n-1 || ydy<0 || ydy>m-1 || map[xdx][ydy]==1 
					|| closed_nodes_map[xdx][ydy]==1))
				{
					// generate a child node
					m0=new node( xdx, ydy, n0->getLevel(), 
								 n0->getPriority());
					m0->nextLevel(i);
					m0->updatePriority(xFinish, yFinish);

					// if it is not in the open list then add into that
					if(open_nodes_map[xdx][ydy]==0)
					{
						open_nodes_map[xdx][ydy]=m0->getPriority();
						pq[pqi].push(*m0);
						// mark its parent node direction
						dir_map[xdx][ydy]=(i+dir/2)%dir;
					}
					else if(open_nodes_map[xdx][ydy]>m0->getPriority())
					{
						// update the priority info
						open_nodes_map[xdx][ydy]=m0->getPriority();
						// update the parent direction info
						dir_map[xdx][ydy]=(i+dir/2)%dir;

						// replace the node
						// by emptying one pq to the other one
						// except the node to be replaced will be ignored
						// and the new node will be pushed in instead
						while(!(pq[pqi].top().getxPos()==xdx && 
							   pq[pqi].top().getyPos()==ydy))
						{                
							pq[1-pqi].push(pq[pqi].top());
							pq[pqi].pop();       
						}
						pq[pqi].pop(); // remove the wanted node
                    
						// empty the larger size pq to the smaller one
						if(pq[pqi].size()>pq[1-pqi].size()) pqi=1-pqi;
						while(!pq[pqi].empty())
						{                
							pq[1-pqi].push(pq[pqi].top());
							pq[pqi].pop();       
						}
						pqi=1-pqi;
						pq[pqi].push(*m0); // add the better node instead
					}
					else delete m0; // garbage collection
				}
			}
			delete n0; // garbage collection
		}
		return ""; // no route found
	}
};

namespace game
{
	class PathFinder
	{
	public:
		static string GetPath(	int start_x,int start_y,int goal_x,	int goal_y,	int amap[ns_path_finder::n][ns_path_finder::m])
		{
			for(int y=0;y<ns_path_finder::m;y++)
			{
				for(int x=0;x<ns_path_finder::n;x++) ns_path_finder::map[x][y]=amap[x][y];
			}
			return ns_path_finder::pathFind(start_x, start_y, goal_x, goal_y);
		}
		static string GetPathWithLastMap(int start_x,int start_y,int goal_x,int goal_y)
		{
			return ns_path_finder::pathFind(start_x, start_y, goal_x, goal_y);
		}
		static std::vector<sf::Vector2u> GetPoints(std::string path,sf::Vector2u start_pos)
		{
			const int size = path.size();
			std::vector<sf::Vector2u> points;	points.reserve(size);
			sf::Vector2u current_pos(start_pos.x,start_pos.y);
			for (int i= 0; i < size; i++)
			{
				char c_dir = path[i];
				int dir = c_dir-48;//atoi(&(c_dir));
				current_pos.x += ns_path_finder::dx[dir];
				current_pos.y += ns_path_finder::dy[dir];
				sf::Vector2u temp_pos(current_pos.x,current_pos.y);
				points.push_back(temp_pos);
			}
			return points;
		}
		static void test()
		{
			clock_t tm;
		tm = clock();

		int map[32][32];
		for (int i= 0; i < 32; i++)
		{
			for (int j = 0; j < 32; j++){map[j][i] = 0;}
		}

		for (int i = 4; i < 28; i++)
		{
			if (i!=22)
			map[i][8] = 1;
		}

		for (int i = 7; i < 17; i++)
		{
			if (i!=11)
			map[11][i] = 1;
		}

		std::string path = PathFinder::GetPath(0,0,31,31,map);
		//std::cout << path <<std::cout <<endl;
		std::vector<sf::Vector2u> points = game::PathFinder::GetPoints(path,sf::Vector2u(0,0));
		//for (int i =0; i < points.size(); i++)
		//{
		//	std::cout <<"x = "<< points[i].x <<"y = "<< points[i].y<<std::endl; 
		//}

		double time_elapsed = double(clock() -tm);
		cout<<"Time to calculate the route (ms): "<<time_elapsed<<endl;


		}
	};
};